/**
 * Created by Samuel Brucksch
 *
 * Compass calibration: http://forum.bildr.org/viewtopic.php?f=29&t=650&start=0
 * Compass code taken from http://bluelemonlabs.blogspot.de/2013/08/arduino-simple-compass-with-hmc5883l.html
 * 
 * and adapted by Samuel Brucksch
 */
#include "compass.h"
#include <inttypes.h>
#include <Wire.h>
#include <math.h>
#include "config.h"
#include <Arduino.h>
#include <EEPROM.h>
#include "servos.h"
#ifdef DEBUG
#include "uart.h"
#endif


void write(int address, int data)
{
  Wire.beginTransmission(HMC5883L_Address);
  Wire.write(address);
  Wire.write(data);
  Wire.endTransmission();
}

#if 0
MagnetometerScaled readScaledAxis(){
  MagnetometerRaw raw = readRawAxis();
  MagnetometerScaled scaled = MagnetometerScaled();
  scaled.XAxis = raw.XAxis * 0.92;
  scaled.YAxis = raw.YAxis * 0.92;
  return scaled;
}
#endif

MagnetometerRaw readRawAxis(){
  MagnetometerRaw raw = MagnetometerRaw();

  Wire.beginTransmission(HMC5883L_Address);
  Wire.write(DataRegisterBegin);
  Wire.endTransmission();

  Wire.beginTransmission(HMC5883L_Address);
  Wire.requestFrom(HMC5883L_Address, 6);

  if(Wire.available() == 6) {
    raw.XAxis = (Wire.read() << 8) | Wire.read();
    uint8_t tmp = Wire.read();
    tmp = Wire.read();
    raw.YAxis = (Wire.read() << 8) | Wire.read();
  }
  Wire.endTransmission();

  return raw;
}

void setMeasurementMode(uint8_t mode){
  write(ModeRegister, mode);
}

void initCompass(){
  // average 8 samples at 75hz
  write(ConfigurationRegisterA, 0x78);
  delay(50);
  write(ConfigurationRegisterB, 0x20);
  delay(20);

  for (int i = 0; i < 100; i++){
    setMeasurementMode(Measurement_Single);
    delay(5);
    readRawAxis();
  }
  write(ConfigurationRegisterA, 0x70);
  setMeasurementMode(Measurement_Continuous);
  
  readCalibration();
}

//int xScaleFactor;
//int yScaleFactor;
int compassXOffset;
int compassYOffset;


void startCompassCalibration(void) {       
  int xMax = 0;
  int yMax = 0;
  int xMin = 0;
  int yMin = 0;
  MagnetometerRaw magraw;
  MagnetometerScaled scaled;
  //rotate tracker 10 seconds
  long time = millis();
  SET_PAN_SERVO_SPEED(PAN_0 + 250);
  while (millis() - time < 10000){
    delay(50);
    magraw = readRawAxis();
    //prevent extraneous high readings from messing with calibration
    if(magraw.XAxis>500 || magraw.XAxis<-500 || magraw.YAxis>500 || magraw.YAxis<-500)
      continue;
  
    if ( magraw.XAxis > xMax ) {
      xMax = magraw.XAxis;
    }
    else if ( magraw.XAxis < xMin ) {
      xMin = magraw.XAxis;
    }
  
    if ( magraw.YAxis > yMax ) {
      yMax = magraw.YAxis;
    }
    else if ( magraw.YAxis < yMin ) {
      yMin = magraw.YAxis;
    }
  }
  SET_PAN_SERVO_SPEED(PAN_0);
#if 0
  calcScaleFactor_Offset();
#else
  compassXOffset = (xMax + xMin)/2;
  compassYOffset = (yMax + yMin)/2;
#endif
  
  #ifdef DEBUG
    char s[5];
    uart_puts("xMax: ");uart_puts(itoa(xMax, s, 10));uart_puts(" xMin: ");uart_puts(itoa(xMin, s, 10));uart_puts(" yMax: ");uart_puts(itoa(yMax, s, 10));uart_puts(" yMin: ");uart_puts(itoa(yMin, s, 10));
    //uart_puts(" xScaleFactor: ");uart_puts(itoa(xScaleFactor, s, 10));uart_puts(" yScaleFactor: ");uart_puts(itoa(yScaleFactor, s, 10));
    uart_puts(" compassXOffset: ");uart_puts(itoa(compassXOffset, s, 10));uart_puts(" compassYOffset: ");uart_puts(itoa(compassYOffset, s, 10));
    uart_putc('\n');
  #endif
  writeCalibration();    //save the calculated x & y offset and scale factor    
}

int getHeading(){
#if 0
  MagnetometerScaled scaled = readScaledAxis();
  if (xScaleFactor != 0){
    scaled.XAxis = (xScaleFactor * scaled.XAxis) + compassXOffset;
  }
  if (yScaleFactor != 0){
    scaled.YAxis = (yScaleFactor * scaled.YAxis) + compassYOffset;
  }
#else
  MagnetometerScaled scaled;
  MagnetometerRaw raw = readRawAxis();
  scaled.XAxis = (raw.XAxis + compassXOffset) * 0.92;
  scaled.YAxis = (raw.YAxis + compassYOffset) * 0.92;
#endif

  double heading = atan2(scaled.YAxis, scaled.XAxis) + DECLINATION;

  if(heading < 0)
    heading += 2*M_PI;

  if(heading > 2*M_PI)
    heading -= 2*M_PI;

  return (int) (heading * 180.0/M_PI);
}

#if 0
void calcScaleFactor_Offset(){
  xScaleFactor = (yMax - yMin)/(xMax - xMin);
  if(xScaleFactor < 1)
    xScaleFactor = 1;

  yScaleFactor = (xMax - xMin)/(yMax - yMin);
  if(yScaleFactor < 1)
    yScaleFactor = 1;

  compassXOffset = ((xMax - xMin)/2 - xMax) * xScaleFactor;
  compassYOffset = ((yMax - yMin)/2 - yMax) * yScaleFactor;
}
#endif

void writeCalibration(){
  int tmpValue;

  #define CHECK_IF_VALUE_NEEDS_TO_BE_SAVED(A, B, C)      \
    tmpValue = (EEPROM.read(C) *256) + EEPROM.read(B);   \
    if(A != tmpValue) {                                  \
      EEPROM.write(B,highByte(A));                       \
      EEPROM.write(C,highByte(A));                       \
    }

  // we'll check the value before writing to save the eeproms live  
  CHECK_IF_VALUE_NEEDS_TO_BE_SAVED(compassXOffset, 0, 1)
  CHECK_IF_VALUE_NEEDS_TO_BE_SAVED(compassYOffset, 2, 3)
#if 0
  CHECK_IF_VALUE_NEEDS_TO_BE_SAVED(xScaleFactor, 4, 5)
  CHECK_IF_VALUE_NEEDS_TO_BE_SAVED(yScaleFactor, 6, 7)
#endif
}

void readCalibration(){
  compassXOffset = (EEPROM.read(1) *256) + EEPROM.read(0);
  compassYOffset = (EEPROM.read(3) *256) + EEPROM.read(2);
#if 0
  xScaleFactor = (EEPROM.read(5) *256) + EEPROM.read(4);
  yScaleFactor = (EEPROM.read(7) *256) + EEPROM.read(6);
#endif
}


